#! /usr/bin/env python
#coding=utf-8
import rospy
import cv2
import tf
from megatron.msg import wr
from geometry_msgs.msg import PoseStamped
rospy.init_node('guard1')
pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
lis = tf.TransformListener()
def gdcb(wr):
    with open('/home/xbot/catkin_ws/src/megatron/src/include/data.txt','w') as f:
        f.write('1')
    lis.waitForTransform('map', 'base_link', rospy.Time(0),rospy.Duration(1.0))
    (t,r) = lis.lookupTransform('map', 'base_link', rospy.Time(0))
    ss = PoseStamped()
    '''header: 
  seq: 1
  stamp: 
    secs: 29
    nsecs: 502000000
  frame_id: "map"
pose: 
  position: 
    x: 5.3907494545
    y: -0.700152397156
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
'''
    ss.header.frame_id = "map"
    ss.pose.position.x = t[0]
    ss.pose.position.y = t[1]
    ss.pose.position.z = t[2]
    ss.pose.orientation.x = r[0]
    ss.pose.orientation.y = r[1]
    ss.pose.orientation.z = r[2]
    ss.pose.orientation.w = r[3]
    pub.publish(ss) #覆盖不掉新的
    print('\nGuard: Optimus...I\'ll report now!\n')
rospy.Subscriber("warn", wr, gdcb, queue_size=10)
rospy.spin()
